/* 
 * coder4Motor.c - The C source file of Orthogonal encoder devices
 * NOTE: This file is based on HAL library of stm32 platform
 *          PWM设备BSP接口实现定义
 *          接口声明功能注释见BSP.h
 *
 *      配置：
 *          0号电机，采用定时器4的CH1和CH2作为输入
 *          1号电机，采用定时器8的CH1和CH2作为输入
 *          都是采用（500*30*4-1）的计数周期，即溢出一次为转4圈
 *      电机&编码器参数：
 *          - 编码器分辨率：500线
 *          - 减速比：30
 *          - 最大转速：300r/min=5r/s @ 12V
 *
 * Copyright (c) 2020-, FOSH Project
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author            Notes           mail
 * 2021-03-04     江榕煜       first version   2373180028@qq.com
**/

#include "BSP.h"

#ifdef __USER_DEBUG__   // 全局调试打印使能

#define __USER_DEBUG__CODER__

#endif
#define __USER_DEBUG__CODER__ //手动启动单项调试打印

//#define CODER_ANALYSE_DIR_SOFT  // 使用软件解析方向
//#define CODER_ANALYSE_DIR_HARD  // 使用硬件解析方向
#define CODER_ANALYSE_DIR_NONE  // 不解析方向

#define CODER0_TIM_HANDLER htim4    // 电机0号定时器
#define CODER1_TIM_HANDLER htim8    // 1号定时器

static int32_t coderPos_Pre[2]= {0};
static TIM_HandleTypeDef * coderHandler[2] = {0};

/** @brief  正交编码器初始化
 *  @note   调用前先初始化定时器设备
 *  @author 江榕煜（2021.4.9）
 *  @param  
 *          None
 *  @retval 
 *          None
**/
void ABcoder_Init(void)
{
    // 假设已经在MX_TIMx_Init中初始化定时器配置
    coderHandler[0] = &CODER0_TIM_HANDLER;
    coderHandler[1] = &CODER1_TIM_HANDLER;
    
    HAL_TIM_Encoder_Start(coderHandler[0],TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start(coderHandler[1],TIM_CHANNEL_ALL);
    
}

/** @brief  计算计数器差
 *  @note
 *  @author 江榕煜（2021.5.27）
 *  @param  
 *      channel(uint32_t)       通道号，从0开始
 *  @retval
 *      deltaSpeed(int32_t)    计数器差值，定采样可代表速度
**/
int32_t ABcoder_getSpeed(uint32_t channel)
{
    static int32_t deltaSpeed =0;
    static int32_t coderPosNow;
    coderPosNow = __HAL_TIM_GET_COUNTER(coderHandler[channel]);
    //__HAL_TIM_SET_COUNTER(coderHandler[channel],0);
    
    #ifdef CODER_ANALYSE_DIR_HARD
    if( __HAL_TIM_IS_TIM_COUNTING_DOWN(coderHandler[channel]) )
    {
        // 正在向下计数，即反转
        if(coderPosNow > coderPos_Pre[channel])
        {
            // 反转溢出
            deltaSpeed = coderPosNow- coderPos_Pre[channel]  - 11*19*200 ;
        }
        else{
            deltaSpeed = coderPosNow - coderPos_Pre[channel];
        }
    }
    else{
        // 正在向上计数，即正转
        if(coderPosNow >= coderPos_Pre[channel])
        {
            deltaSpeed = coderPosNow - coderPos_Pre[channel];
        }
        else{
            // 显然正转溢出
            deltaSpeed = coderPosNow + 11*19*200 - coderPos_Pre[channel];
        }
    }
    #endif
    
    #ifdef CODER_ANALYSE_DIR_SOFT
    if(coderPos_Pre[channel] >= (Motor_ABcoder_counter+1)/2)    // 位于计数周期上半域
    {
        if( ((coderPos_Pre[channel] - (Motor_ABcoder_counter+1)/2)<=coderPosNow)
            &&
            (coderPosNow<coderPos_Pre[channel])  )
        {
            // 计数器在下计数
            deltaSpeed = coderPosNow - coderPos_Pre[channel];
        }
        else
        {
            // 上计数溢出
            deltaSpeed = coderPosNow + Motor_ABcoder_counter+1 - coderPos_Pre[channel];
        }
    }
    else    // 位于计数周期下半域
    {
        if( (coderPos_Pre[channel]<=coderPosNow)
            &&
            (coderPosNow<=(coderPos_Pre[channel]+(Motor_ABcoder_counter+1)/2)) )
        {
            // 计数器在上计数
            deltaSpeed = coderPosNow - coderPos_Pre[channel];
        }
        else
        {
            // 下计数溢出
            deltaSpeed = coderPosNow- coderPos_Pre[channel]  - (Motor_ABcoder_counter+1)/2 ;
        }
    }
    #endif
    
    #ifdef CODER_ANALYSE_DIR_NONE
    deltaSpeed = coderPosNow - coderPos_Pre[channel];
    
    
    if(deltaSpeed> (Motor_ABcoder_counter)/2)
        deltaSpeed -= (Motor_ABcoder_counter);
    else if(deltaSpeed< -(Motor_ABcoder_counter)/2)
        deltaSpeed += (Motor_ABcoder_counter);
    
    #endif
    
    coderPos_Pre[channel] = coderPosNow;  // 记忆计数器
    return deltaSpeed;
}

